#include <0A_robot_devices.h>

#include "ps2.h"

Leg leg_1 = {
    .leg_length = 86.57f,

    .start_angle_yaw = 0.0f,
    .start_angle_pitch = 0.0f,
    .start_angle_roll = 0.0f,
};

Leg leg_2 = {
    .leg_length = 87.04f,

    .start_angle_yaw = 0.0f,
    .start_angle_pitch = 14.03f,
    .start_angle_roll = 0.0f,
};

Leg leg_3 = {
    .leg_length = 196.75f,

    .start_angle_yaw = 0.0f,
    .start_angle_pitch = -58.38f,
    .start_angle_roll = 0.0f,
};

LegGroup leg_group_1 = {
    .LG_servos =
        {
            {.ID = 23},
            {.ID = 1},
            {.ID = 2},
        },
    .deg_offset = {15.0f, -30.0f, 25.0f},
};
LegGroup leg_group_2 = {
    .LG_servos =
        {
            {.ID = 3},
            {.ID = 4},
            {.ID = 5},
        },
    .deg_offset = {5.0f, 20.0f, -30.0f},

};
LegGroup leg_group_3 = {
    .LG_servos =
        {
            {.ID = 6},
            {.ID = 7},
            {.ID = 8},
        },
    .deg_offset = {-20.0f, 30.0f, 0.0f},

};
LegGroup leg_group_4 = {
    .LG_servos =
        {
            {.ID = 9},
            {.ID = 10},
            {.ID = 11},
        },
    .deg_offset = {0.0f, -60.0f, -60.0f},

};
LegGroup leg_group_5 = {
    .LG_servos =
        {
            {.ID = 12},
            {.ID = 22},
            {.ID = 14},
        },
    .deg_offset = {0.0f, -30.0f, -35.0f},

};
LegGroup leg_group_6 = {
    .LG_servos =
        {
            {.ID = 15},
            {.ID = 16},
            {.ID = 17},
        },
    .deg_offset = {0.0f, -60.0f, 40.0f},
};

LegGroup *leg_group_array[6] = {&leg_group_1, &leg_group_6, &leg_group_5,
                                &leg_group_2, &leg_group_3, &leg_group_4};

Servo_Config test_servo_config = {
    .duty_percent_max = 12.5f,    // 舵机最大占空比
    .duty_percent_min = 2.5f,     // 舵机最小占空比
    .servo_degree_max = 270,      // 舵机最大转动角度
    .servo_degree_min = 0,        // 舵机最小转动角度
    .servo_htim = &htim1,         // 舵机定时器
    .tim_channel = TIM_CHANNEL_1  // 舵机定时器通道
};
Servo test_servo;

Pump air_pump_1;
Pump air_pump_2;
Pump air_pump_3;
Pump air_pump_4;
Pump air_pump_5;
Pump air_pump_6;

PumpGroup pump_group_a;
Pump *group_a_array[3] = {&air_pump_1, &air_pump_3, &air_pump_5};

PumpGroup pump_group_b;
Pump *group_b_array[3] = {&air_pump_2, &air_pump_4, &air_pump_6};

PumpGroup pump_group_all;
Pump *group_all_array[6] = {&air_pump_1, &air_pump_6, &air_pump_5,
                            &air_pump_2, &air_pump_3, &air_pump_4};

PWM glue_pusher_ctl;

void device_init(void) {
    // PS2初始化
    PS2_SetInit();

    // 初始化舵机组
    LG_init(&leg_group_1, &huart2);
    LG_init(&leg_group_2, &huart2);
    LG_init(&leg_group_3, &huart2);
    LG_init(&leg_group_4, &huart2);
    LG_init(&leg_group_5, &huart2);
    LG_init(&leg_group_6, &huart2);

    // 初始化测试舵机
    Servo_init(&test_servo, &test_servo_config);

    // 初始化PWM控制的气泵阀门
    Pump_init(&air_pump_1, &htim2, TIM_CHANNEL_1);
    Pump_init(&air_pump_2, &htim2, TIM_CHANNEL_2);
    Pump_init(&air_pump_3, &htim2, TIM_CHANNEL_3);
    Pump_init(&air_pump_4, &htim2, TIM_CHANNEL_4);
    Pump_init(&air_pump_5, &htim3, TIM_CHANNEL_1);
    Pump_init(&air_pump_6, &htim3, TIM_CHANNEL_2);

    // 初始化气泵组
    PumpGroup_init(&pump_group_a, group_a_array, 3);
    PumpGroup_init(&pump_group_b, group_b_array, 3);
    PumpGroup_init(&pump_group_all, group_all_array, 6);

    // 初始化胶水推杆
    PWM_init(&glue_pusher_ctl, &htim3, TIM_CHANNEL_3);
}
